#include "predict.h"
#include "visual.h"
#include "yaml_config.h"
#include "opencv.hpp"
#include <librealsense2/rs.hpp>
#include "task.h"

void Task_infer(RobotController &robot_a1)
{
  mtx.lock();
  /* 定义相关变量 */
  Mat pred, added;
  double start, end, time;
  const int width = 640;
  const int height = 480;
  const int fps = 30;
  Mat H = calibration();

  /* 加载模型 */
  string yaml_path = "../Model/deploy.yaml";
  YamlConfig yaml_config = load_yaml(yaml_path);
  auto predictor = create_predictor(yaml_config);

  /* 配置Realsense */
  rs2::context ctx;
  auto list = ctx.query_devices();
  rs2::device dev = list.front();
  rs2::frameset frames;
  rs2::pipeline pipe;
  rs2::config cfg;
  cfg.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_BGR8, fps);
  pipe.start(cfg); // 指示管道使用所请求的配置启动流
  mtx.unlock();

  while (1)
  {
    start = static_cast<double>(getTickCount());

    /* 1.预测 */
    frames = pipe.wait_for_frames(); // 等待所有配置的流生成框架
    rs2::frame color_frame = frames.get_color_frame();
    Mat src(Size(width, height), CV_8UC3, (void *)color_frame.get_data(), Mat::AUTO_STEP);
    Run_Predict(predictor, yaml_config, src, pred);

    /* 2.可视化 */
    added = addedImage(src, pred);
    robot_a1.get_Line(pred,H);
    //robot_a1.midLine.fitting();

    mtx.lock();
    robot_a1.calculate_speed();
    mtx.unlock();

    // robot_a1.midLine.draw(added);
    // robot_a1.leftLine.draw(added);
    // robot_a1.rightLine.draw(added);
    // warpPerspective(added, added, H, Size(640, 480)); //转成鸟瞰图
    // darw_info(added);

    end = static_cast<double>(getTickCount());
    time = (end - start) / getTickFrequency();
    cout << "FPS " << (int)(1 / time) << endl;

    // imshow("pred", added);
    // waitKey(1);

    this_thread::sleep_for(chrono::milliseconds(10));
  }
}
